//传感器定义
#define CrashSensor A0 //碰撞传感器 --> A0端口
#define IRSensor1 A1 //车头左侧红外反射传感器 --> A1端口
#define IRSensor2 A2 //车头右侧红外反射传感器 --> A2端口
#define Trig 13 //HC-SR04超声波传感器的引脚Trig --> D13端口
#define Echo 12 //HC-SR04超声波传感器的引脚Echo--> D12端口

//执行器定义
#define SPEED 6
#define SPEED_2 9
#define SPEED_3 10
#define SPEED_4 11
#define BUZZER 3

int Motor_speed = 250; //定义电机转速（车速）
float crashDistance; //定义距离变量
float echoTime; //定义回波等待时间

void setup() {
  pinMode(CrashSensor,INPUT); //碰撞传感器初始化
  pinMode(IRSensor1,INPUT); //左侧红外反射传感器初始化
  pinMode(IRSensor2,INPUT); //右侧红外反射传感器初始化
  pinMode(Echo,INPUT); //HC-SR04超声波传感器的Echo引脚初始化
  
  //电机初始化
  pinMode(SPEED,OUTPUT);
  pinMode(SPEED_2,OUTPUT);
  pinMode(SPEED_3,OUTPUT);
  pinMode(SPEED_4,OUTPUT);

  pinMode(BUZZER,OUTPUT); //蜂鸣器初始化
  pinMode(Trig,OUTPUT); //HC-SR04超声波传感器的Trig引脚初始化

  Serial.begin(9600); //初始化串口监视器 --> 波特率设置为9600
}

void distanceCal(){
  //给超声波传感器的Trig发送一个低高低的微秒级时间脉冲,触发测距
  digitalWrite(Trig, LOW); //给Trig发送一个低电平
  delayMicroseconds(2);    //等待2微秒
  digitalWrite(Trig,HIGH); //给Trig发送一个高电平
  delayMicroseconds(10);    //等待10微秒
  digitalWrite(Trig, LOW); //给Trig发送一个低电平

  echoTime = float(pulseIn(Echo, HIGH)); //读取回波等待时间
  //如有信号返回， Echo 引脚会输出高电平，高电平持续的时间就是超声波从发射到返回的时间。
  crashDistance = (echoTime*17)/1000;
  //超声波在空气中的传播速度是:340m/1s 换算成 34000cm / 1000000μs => 34 / 1000
  //因为发送到接收,实际是相同距离走了2回,所以要除以2
  //距离(厘米)  =  (回波时间 * (34 / 1000)) / 2 =  (回波时间 * 17)/ 1000

}

void singalPrint(){    //传感器信号读取 --> 在串口监视器上显示
  Serial.print("左侧红外反射传感器数值（1代表正常，0代表检测到道路边缘）："); //打印左侧红外反射传感器数值的提示信息
  Serial.println(digitalRead(IRSensor1));
  
  Serial.print("右侧红外反射传感器数值（1代表正常，0代表检测到道路边缘）：");//打印右侧红外反射传感器数值的提示信息
  Serial.println(digitalRead(IRSensor2));
  
  Serial.print("碰撞传感器情况数值（1代表检测到碰撞，0代表未检测到碰撞）："); //打印碰撞传感器数值的提示信息
  Serial.println(digitalRead(CrashSensor)); //读取碰撞传感器数值

  Serial.print("回波等待时间：");
  Serial.print(echoTime);//串口输出等待时间的原始数据
  Serial.print(" | | 车头与障碍物距离： ");
  Serial.print(crashDistance);//串口输出距离换算成cm的结果
  Serial.println("cm");
  Serial.println("------------------------------"); //打印分隔符
  delay(200); //延迟200毫秒 --> 每0.2秒滚动刷新一次
}


void Motor_control(int Speed_L,int Speed_R) //电机控制
{
  if(Speed_L >0){
    //车左2轮反转
    analogWrite(SPEED_3,LOW); 
    analogWrite(SPEED_4,Speed_L);
  }
  else{
    //车左2轮正转
    analogWrite(SPEED_3,-Speed_L);
    analogWrite(SPEED_4,LOW);
  }
  if(Speed_R >0){
    //车右2轮正转
    analogWrite(SPEED,LOW); 
    analogWrite(SPEED_2,Speed_R); 
  }
  else{
    //车右2轮反转
    analogWrite(SPEED,-Speed_R);
    analogWrite(SPEED_2,LOW); 
  }
}

void Move_forward(){// RGB LED灯亮绿
  Motor_control(-Motor_speed,Motor_speed); //前进
}

void Move_back(){ // RGB LED灯亮粉色
  Motor_control(Motor_speed,-Motor_speed); //后退
}

void Turn_left(){ // RGB LED灯亮黄色
  Motor_control(-Motor_speed,-Motor_speed); //差速转向：原地左转
}

void Turn_Right(){// RGB LED灯亮蓝色
  Motor_control(Motor_speed,Motor_speed); //差速转向：原地右转
}

void buzzerWork(){
  tone(BUZZER,523,200);   //将蜂鸣器模块对应端口设为高电平
  delay(200);
  noTone(BUZZER);
  delay(200);
}

void crashDet(){ //碰撞检测
//  if (digitalRead(CrashSensor) == 1){ //如果检测到碰撞
//    Motor_control(0,0); //电机停转，同时RGB LED灯亮白色
//    buzzerWork(); //蜂鸣器开始工作
//  }
//  else //如果未检测到碰撞
//  {
//    Move_forward(); //前进
//  }
  while(digitalRead(CrashSensor) == 1){
    Motor_control(0,0); //电机停转，同时RGB LED灯亮白色
    buzzerWork(); //蜂鸣器开始工作
  }
}

void crashAvoid(){ //避障
  if (crashDistance <= 20.0){
    Move_back();
    delay(500);
    Turn_Right();
    delay(650);
    Move_forward();
    delay(2000);
    Turn_left();
    delay(650);
    Move_forward();
    delay(100);
    
  }
//  else if(digitalRead(CrashSensor) == 1){
//    Motor_control(0,0); //电机停转，同时RGB LED灯亮白色
//    buzzerWork(); //蜂鸣器开始工作
//    
//  }

  else{
    Move_forward();
  }
    while(digitalRead(CrashSensor) == 1){
    Motor_control(0,0); //电机停转，同时RGB LED灯亮白色
    buzzerWork(); //蜂鸣器开始工作
  }
}


void autoDrive(){
  if(digitalRead(IRSensor1)==1 && digitalRead(IRSensor2)==1){
    Move_forward(); //前进
  }
  //如果车头左侧红外反射传感器IRsensor1检测到道路边缘
  else if(digitalRead(IRSensor1)==0 && digitalRead(IRSensor2)==1){
    Move_back(); //后退
    delay(300);  //延迟0.3秒
    Turn_Right(); //原地右转
    delay(250);  //延迟0.25秒
  }
  //如果车头右侧红外反射传感器IRsensor2检测到道路边缘
  else if(digitalRead(IRSensor1)==1 && digitalRead(IRSensor2)==0){
    Move_back(); //后退
    delay(300);  //延迟0.3秒
    Turn_left(); //原地左转
    delay(250);  //延迟0.25秒
  }
  // else if 等待改进
  
}

void loop() {
  distanceCal();//距离计算
  singalPrint(); //打印传感器数值信息
  crashAvoid(); //避障
  crashDet(); //碰撞检测
  autoDrive(); //自动行驶
}
